#include "motor_ctrl.h"

PID_Gather_t m[3];
Axis3f pos;
Traj_Typedef Traj_ricexy;  //加减速调用
Traj_Typedef Traj_ricez;  //加减速调用

float start_x, start_y, start_z;
float error_x, error_y, error_z;
float x_now, y_now, z_now;
float angle_arror=0;
float angle_init=0;

float tja_xy = 1450, tja_z = 1300;
//float tja_xy = 600, tja_z = 800;
/**
 * @brief  初始化电机
 * @param  
 * @retval 无
 */
void can_motor_init(void)
{
	//位置控制
	for (uint8_t i=0; i < 3; i++) {
		pid_init(&m[i].out, 0.10f, 0, 0, 20000, 20000);
		pid_init(&m[i].in, 15, 2, 0, 20000, 20000);
	}
}

/**
 * @brief  看看结束了没
 * @param  
 * @retval 无
 */
uint8_t xyend_GetEndFlag(void)
{
	return TRAJ_getEndFlag(Traj_ricexy);
}

/**
 * @brief  看看结束了没
 * @param  
 * @retval 无
 */
uint8_t zend_GetEndFlag(void)
{
	return TRAJ_getEndFlag(Traj_ricez);
}

/**
 * @brief  设置xy坐标
 * @param  
 * @retval 无
 */
void can_motor_setxy(float x, float y)
{
	if (y > 145) {
		y = 145;
	}
	
	if (y < 0) {
		y = 0;
	}
	
	if (x > 240) {
		x = 240;
	}
	
	if (x < 0) {
		x = 0;
	}
	
	if (!TRAJ_getEndFlag(Traj_ricexy)) {
		return;
	}
	
	float dis = TrackPoint_getManhattanDis(x_now, x, y_now, y);
	
	taskENTER_CRITICAL();
	TRAJ_plan(&Traj_ricexy, dis, 0, tja_xy, 200, 200); //加减速初始化
	taskEXIT_CRITICAL();
	
	start_x = x_now;
	start_y = y_now;
	
	error_x = x - start_x;
	error_y = y - start_y;
}

/**
 * @brief  设置z坐标
 * @param  
 * @retval 无
 */
void can_motor_setz(float z)
{
	if (z > 50) {
		z = 50;
	}

	if (z < -0) {
		z = -0;
	}
	
	if (!TRAJ_getEndFlag(Traj_ricez)) {
		return;
	}
	
	float dis = z - z_now;
	
	taskENTER_CRITICAL();
	TRAJ_plan(&Traj_ricez, dis, 0, tja_z, 600, 600); //加减速初始化
	taskEXIT_CRITICAL();
	
	start_z = z_now;
	
	error_z = dis;
}

/**
 * @brief  
 * @param  
 * @retval 无
 */
void can_motor_setxy_ForPark(float x, float y)
{
	Axis3f pos;
	
	//坐标系围绕4点旋转
	float x1 = x - play_piece_point[4].x;
	float y1 = y - play_piece_point[4].y;
	
	float angle = get_angle();
	
	float x2 = cosf(angle) * x1 + sinf(angle) * y1;
	float y2 =-sinf(angle) * x1 + cosf(angle) * y1;
	
	x2 = x2 + play_piece_point[4].x;
	y2 = y2 + play_piece_point[4].y;
	
	can_motor_setxy(x2, y2);
}

/**
 * @brief  闭环电机
 * @param  
 * @retval 无
 */
void can_motor_task(void)
{
	float out[3];
	
	TRAJ_eval(&Traj_ricexy, super_dt);  //加减速计算
	TRAJ_eval(&Traj_ricez,  super_dt);  //加减速计算
	
	/* 计算速度 */
	float k  = TRAJ_ModelScale_ForDis(Traj_ricexy);
	float kz = TRAJ_ModelScale_ForDis(Traj_ricez);
	
	/* 施加量 */
	pos.x = start_x + (error_x * k);
	pos.y = start_y + (error_y * k);
	pos.z = start_z + (error_z * kz);
	
	/* id */
	uint16_t id[3] = {
		CAN_Motor_ID1,
		CAN_Motor_ID2,
		CAN_Motor_ID3
	};
	
	//计算速度反馈
	x_now =  -M2006_CntToAngle(CAN_Motor_getTotalAngle(id[0])) * 28.130f;
	y_now =  -M2006_CntToAngle(CAN_Motor_getTotalAngle(id[1])) * 28.130f;
	z_now =   M2006_CntToAngle(CAN_Motor_getTotalAngle(id[2])) * 28.130f;
	
	float set_motor_out[3] = {
		-M2006_AngleToCnt(pos.x / 28.130f),
		-M2006_AngleToCnt(pos.y / 28.130f),
		 M2006_AngleToCnt(pos.z / 28.130f),
	};
		
	
	/* xy */
	out[0] = pid_calculate_position(&m[0].out, set_motor_out[0], CAN_Motor_getTotalAngle(id[0]), 0.001f);
	out[1] = pid_calculate_position(&m[1].out, set_motor_out[1], CAN_Motor_getTotalAngle(id[1]), 0.001f);
	
	/* z */
	out[2] = pid_calculate_position(&m[2].out, set_motor_out[2], CAN_Motor_getTotalAngle(id[2]), 0.001f);
	
	
	for (uint8_t i=0; i < 3; i++) {
		out[i] = PID_Compute_Increm(&m[i].in, out[i], CAN_Motor_getSpeed(id[i]));
	}
	
	for (uint8_t i=0; i < 3; i++) {
		CAN_Motor_setCurrent(id[i], out[i]);
	}
	
	send_can1_motor_current();
}

/******************* (C) COPYRIGHT 2022 FUYEBING*****END OF FILE************/
